﻿using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.FEZ;
using GHIElectronics.NETMF.Net;
using GHIElectronics.NETMF.Net.Sockets;
using GHIElectronics.NETMF.Net.NetworkInformation;
using GHIElectronics.NETMF.Hardware;
using System.Text;
using Ascended.System;
using Socket = GHIElectronics.NETMF.Net.Sockets.Socket;



public class Program
{
    //FEZ_Components.DistanceDetector detector = new FEZ_Components.DistanceDetector(FEZ_Pin.AnalogIn.An1, FEZ_Components.DistanceDetector.SharpSensorType.GP2D120);


    public static byte[] sendData(string send)
    {
        byte[] bytesToSend = Encoding.UTF8.GetBytes(send);

        return bytesToSend;
    }

    //public string objectClose()
    //{
    //    float value = detector.GetDistance_cm();
    //    if (value < 9)
    //    {
    //        return "Too Close";
    //    }
    //    else
    //    {
    //        return null;
    //    }
    //}


    public static void Main()
    {
        byte[] bytesToSend;
        String msg = "Got It";
        String tstMsg;
        string toUse;
        sbyte convert = 0;
        sbyte convert2 = 0;
     
        Program newInstance = new Program();
        //FEZ_Components.ServoMotor turnServo = new FEZ_Components.ServoMotor(FEZ_Pin.Digital.Di6);

        FEZ_Shields.DCMotorDriver.Initialize();

        byte[] ip = { 192, 168, 1, 250 };
        byte[] subnet = { 255, 255, 255, 0 };
        byte[] gateway = { 192, 168, 1, 1 };
        byte[] mac = { 0x00, 0x26, 0x1C, 0x7B, 0x29, 0xE8 };

        WIZnet_W5100.Enable(SPI.SPI_module.SPI1, (Cpu.Pin)FEZ_Pin.Digital.Di10, (Cpu.Pin)FEZ_Pin.Digital.Di9, true); // WIZnet interface setting on FEZ Connect
        NetworkInterface.EnableStaticIP(ip, subnet, gateway, mac);
        NetworkInterface.EnableStaticDns(new byte[] { 192, 168, 1, 1 });

        Socket mySocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
        IPAddress ServerIP = new IPAddress(new byte[] { 192, 168, 1, 104 });
        IPEndPoint ServerEndPoint = new IPEndPoint(ServerIP, 2000);



        Int32 incomingValue;
        int incomingTurnServo;

        bytesToSend = sendData(msg);

        //byte[] bytesToSend = Encoding.UTF8.GetBytes(msg);


        while (true)
        {
            mySocket.SendTo(bytesToSend, bytesToSend.Length, SocketFlags.None, ServerEndPoint);

            while (mySocket.Poll(2000, SelectMode.SelectRead))
            {

                if (mySocket.Available > 0)
                {
                    byte[] inBuf = new byte[mySocket.Available];
                    EndPoint recEndPoint = new IPEndPoint(IPAddress.Any, 0);
                    mySocket.ReceiveFrom(inBuf, ref recEndPoint);

                    

                    toUse = new String(Encoding.UTF8.GetChars(inBuf));

                     FEZ_Shields.DCMotorDriver.Move(convert, convert2);

                    //string distance = newInstance.objectClose();

                    if (toUse.IndexOf("Y") != -1)
                    {
                        char[] remove = { 'Y' };

                        string trimmed = toUse.Trim(remove);

                        //Debug.Print(trimmed);

                        incomingValue = ConvertString.ToInt32(trimmed);
                        convert = (sbyte)incomingValue;

                      
                        //FEZ_Shields.DCMotorDriver.Move(convert, convert);
                    }


                    if (toUse.IndexOf("X") != -1)
                    {
                        char[] remove = { 'X' };

                        string trimmed = toUse.Trim(remove);

                        incomingTurnServo = ConvertString.ToInt32(trimmed);

                        convert2 = (sbyte)incomingTurnServo;

                        //turnServo.SetPosition(convert);
                    }

                    //if (distance != null) 
                    //{
                    //    sendData(distance);
                    //}

                    if (!recEndPoint.Equals(ServerEndPoint))// Check if the received packet is from the 192.168.0.2

                        continue;

                }
            }
        }
    }
}



